﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using AForge.Robotics.Surveyor;
using System.Threading;
using System.Drawing;
using System.Windows;
using System.Windows.Forms;

namespace AForgeNetController
{
    //public sealed class AforgeNetFactory
    //{
    //    static readonly SVS svs = new SVS();

    //    static AforgeNetFactory()
    //    {
    //    }

    //    AforgeNetFactory()
    //    {
    //    }

    //    public static SVS SVS
    //    {
    //        get
    //        {
    //            return svs;
    //        }
    //    }
    //}
    
    public class AForgeNetController
    {
        #region Private Fields
        //private static AforgeNetFactory svs;
        private static SVS _singleController;
        const string _defaultIpAddress = "169.254.0.10";
        //private SVS svs;
        

        #endregion

        #region Constructor
        /// <summary>
        ///  Constructs a new instance of the AForgeNetController class.
        /// </summary>
        public AForgeNetController()
        {
            if (_singleController == null)
            {
                _singleController = new SVS();
            }

            //return _singleController;
        }
        #endregion

        #region Public Properties

        /// <summary>
        /// Indicates whether the connection
        /// </summary>
        /// <returns>bool</returns>
        public bool IsConnected
        {
            get
            {
                if (_singleController.IsConnected)
                {
                    return true;
                }
                return false;
            }
        }

        #endregion

        #region Public Methods


        /// <summary>
        /// Creates, connects and returns a singleton instance of the AForgeNetController class
        /// </summary>
        //public static SVS AForgeNetFactory()
        //{
        //    if (_singleController == null)
        //    {
        //        _singleController = new SVS();
        //    }

        //    return _singleController;
        //}

        /// <summary>
        /// Opens the port and establishes the connection with the robot
        /// </summary>
        public void Connect(string ipAddress)
        {
            bool connected = false;

            while (!connected)
            {
                try
                {
                    // connect to the SVS board
                    _singleController.Connect(ipAddress);

                }
                catch (Exception ex)
                {
                    MessageBox.Show(ex.Message + " Click OK, to try again.");
                }

                Thread.Sleep(250);

                if (_singleController.IsConnected)
                {
                    connected = true;
                }
            }

        }

        /// <summary>
        /// Run the motors of the robot for a fixed time interval.
        /// </summary>
        /// <param name="leftSpeed"></param>
        /// <param name="rightSpeed"></param>
        /// <param name="duration"></param>
        public void Motor(int leftSpeed, int rightSpeed, int duration)
        {
            duration = (duration / 10);

            try
            {
                _singleController.RunMotors(leftSpeed, rightSpeed, duration);
            }
            catch (Exception ex)
            {

            }
        }

        /// <summary>
        /// Send the robot predefined motor controls.
        /// </summary>
        /// <param name="command"></param>
        public void ControlMotors(SRV1.MotorCommand command)
        {
            try
            {
                _singleController.ControlMotors(command);
            }
            catch(Exception ex)
            {

            }
        }

        /// <summary>
        /// Set the quality for the camera image capture
        /// </summary>
        /// <param name="quality"></param>
        public void SetQuality(int quality)
        {
            try
            {
                _singleController.SetQuality(quality);
            }
            catch (Exception ex)
            {

            }
        }

        /// <summary>
        /// Set the resolution for the camera image capture
        /// </summary>
        /// <param name="resolution"></param>
        public void SetResolution(SRV1.VideoResolution resolution)
        {
            try
            {
                _singleController.SetResolution(resolution);
            }
            catch (Exception ex)
            {

            }
        }

        public SRV1Camera GetCamera(SVS.Camera camera)
        {
            return _singleController.GetCamera(camera);
        }

        public Bitmap GetLeftImage()
        {
            return _singleController.GetImage(SVS.Camera.Left);
        }

        public Bitmap GetRightImage()
        {
            return _singleController.GetImage(SVS.Camera.Right);
        }

        /// <summary>
        /// Opens the port and establishes the connection with the robot
        /// </summary>
        public void Connect()
        {
             bool connected = false;

             while (!connected)
             {
                 try
                 {
                     // connect to the SVS board
                     _singleController.Connect(_defaultIpAddress);
                 }
                 catch (Exception ex)
                 {

                 }

                 Thread.Sleep(250);

                 if (_singleController.IsConnected)
                 {
                     connected = true;
                 }
             }

        }

        /// <summary>
        /// Disconnects from the SRV1 robot.
        /// </summary>
        public void Disconnect()
        {
            try
            {
                // connect to the SVS board
                _singleController.Disconnect();
            }
            catch (Exception ex)
            {

            }

        }

        /// <summary>
        /// Stop the motors on the robot.
        /// </summary>
        public void StopMotors()
        {
            try
            {
                _singleController.StopMotors();
            }
            catch (Exception ex)
            {

            }
        }

        /// <summary>
        /// Get the software version of the robot.
        /// </summary>
        /// <returns>string</returns>
        public string GetVersion()
        {
            string version = string.Empty;
            
            try
            {
                version = _singleController.GetVersion();
            }
            catch (Exception ex)
            {

            }
            return version;
        }        

        #endregion
    }
}
